-- Import Lib
local Vec = dofile("vector.lua")
local ScreenBuffer = dofile("screen_buffer.lua")
local Hub = dofile("hub.lua")

-- Config
local yaw_off = 0
local acclerate = 40


-- Init Ref
local monitor = peripheral.find("monitor") or Hub.wrap(Hub.getPeripherals("monitor")[1] or "NOSCREEN")[1]
local ship = peripheral.find("EngineController") or Hub.wrap(Hub.getPeripherals("EngineController")[1] or "NOSHOP")[1]
local joy = peripheral.find("tweaked_controller") or Hub.wrap(Hub.getPeripherals("tweaked_controller")[1] or "NOJOY")[1]



-- Init Cache
local SBuffer = ScreenBuffer:new(9,5)

local monitorData = {
    w = 0,
    h = 0
}

local uiData = {
    north_bar = {
        x = 0, y = 0,
        half_w = 24
    },
    
    roll_bar = {
        x = 0, y = 0,
        w = 10, h = 10,
    },

    state_bar = {
        x = 3, y = 0
    }

}

local shipDataCached = {
    yaw = 0,
    mass = 0,
    pos = Vec.Zero(),
    velocity = 0
}

yaw_off = yaw_off / 180 * math.pi

local ctrlCaches = {
    yaw_ctrl = 0,
    speed_ctrl = 0,
    key_up = false,
    key_down = false,
    key_r = false,

    key_r_last = false,
    brake = true
}

-- Init Resources
local uiSprite = {
    north_bar_0 =      "W                    N                    E                    S                    W                    N                    E                    S                    W                    N                    E                    S                    W",
    --north_bar_1 = "|      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |      |",
    north_bar_1 =      "|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|------|      |",
    
    north_bar_2 =      "|                    |                    |                    |                    |                    |                    |                    |                    |                    |                    |                    |                    |",
    north_bar_border = ""
}


local function setupScreen()
    if monitor ~= nil then
        monitor.setTextScale(0.5)
        monitorData.w, monitorData.h = monitor.getSize()
        SBuffer:resize(monitorData.w, monitorData.h)

        uiData.north_bar.y = 2
        uiData.north_bar.x = math.ceil(monitorData.w / 2) - uiData.north_bar.half_w
        uiData.state_bar.x = 2
        uiData.state_bar.y = monitorData.h - 2

        uiSprite.north_bar_border = "|"..string.rep(" ", uiData.north_bar.half_w * 2 + 1).."|"
    end
end

local function getScreen()
    if monitor == nil or monitor.getSize() == nil then
        monitor = nil
        monitor = peripheral.find("monitor") or Hub.wrap(Hub.getPeripherals("monitor")[1] or "NOSCREEN")[1]
        setupScreen()
    end
    return monitor
end

local function getJoy()
    if joy == nil or joy.getButton(1) == nil then
        joy = peripheral.find("tweaked_controller") or Hub.wrap(Hub.getPeripherals("tweaked_controller")[1] or "NOJOY")[1]
    end
    return joy
end


local function flushControlCache()
    if getJoy() == nil then
        ctrlCaches.yaw_ctrl = 0
        ctrlCaches.speed_ctrl = 0
        ctrlCaches.key_up = false
        ctrlCaches.key_down = false
        ctrlCaches.key_r = false
        ctrlCaches.brake = true
        return 
    end

    ctrlCaches.yaw_ctrl = joy.getAxis(1) or 0
    ctrlCaches.speed_ctrl = -joy.getAxis(2) or 0
    ctrlCaches.key_up = joy.getButton(12) or false
    ctrlCaches.key_down = joy.getButton(14) or false
    ctrlCaches.key_r = joy.getButton(9) or false

    if ctrlCaches.key_r and not ctrlCaches.key_r_last then
        ctrlCaches.brake = not ctrlCaches.brake
    end

    if not joy.hasUser() then
        ctrlCaches.brake = true
    end

    ctrlCaches.key_r_last = ctrlCaches.key_r
end

local function PhysTick(physImpl)
    local data = physImpl.getInertia()
    local pose = physImpl.getPoseVel()

    local velocity = pose.velocity
    local velocity_num = Vec.Length(velocity)
    local _omega = Vec.Negative(pose.omega)
    --local pos = pose.pos
    --local rot = pose.rot
    local up_vec = pose.up
    local yaw = pose.yaw + yaw_off

    shipDataCached.velocity = velocity_num
    shipDataCached.pos = pose.pos
    shipDataCached.mass = data.mass
    shipDataCached.yaw = yaw

    -- input
    flushControlCache()

    local high_ctrl = 0
    if ctrlCaches.key_up then
        high_ctrl = high_ctrl+1
    end
    if ctrlCaches.key_down then
        high_ctrl = high_ctrl-1
    end

    local tensor = data.momentOfInertiaTensor[1][1]
    -- gyro
    local torque = Vec.Normalize(Vec.Cross(up_vec, Vec.AxisY()))
    local angle = Vec.Angle(up_vec, Vec.AxisY()) * 40

    torque = Vec.MulNum(torque, angle * tensor)
    --torque = physImpl.transformOmega(torque.x, torque.y, torque.z)
    ship.applyInvariantTorque(torque.x, torque.y, torque.z)
    --print(torque.x, torque.y, torque.z)

    local rot_damping = 20
    if ctrlCaches.brake then
        rot_damping = 40
    end

    --local _torque = physImpl.transformOmega(_omega.x, _omega.y, _omega.z)
    local _torque = Vec.MulNum(_omega, rot_damping * tensor)
    ship.applyInvariantTorque(_torque.x, _torque.y, _torque.z)

    if ctrlCaches.yaw_ctrl ~= 0 then
        local tmp = math.max(velocity_num, 20) * 0.4
        torque = Vec.MulNum(ship.getFaceVector(), ctrlCaches.speed_ctrl * ctrlCaches.yaw_ctrl * tmp * tensor)
        --torque = physImpl.transformOmega(torque.x, torque.y, torque.z)
        ship.applyInvariantTorque(torque.x, torque.y, torque.z)
    end

    -- yaw control
    torque = Vec.MulNum(Vec.AxisY(), -ctrlCaches.yaw_ctrl * 40 * tensor)
    --torque = physImpl.transformOmega(torque.x, torque.y, torque.z)
    ship.applyInvariantTorque(torque.x, torque.y, torque.z)

    -- hover power
    ship.applyInvariantForce(0, 10 * data.mass, 0)

    local damping = data.mass * 5

    if ctrlCaches.brake then
        damping = damping * 3
    end

    local damping_force = Vec.MulNum(velocity, -damping)

    if ctrlCaches.speed_ctrl ~= 0 then
        damping_force.x = damping_force.x * 0.5
        damping_force.z = damping_force.z * 0.5
    end

    if high_ctrl then
        damping_force.y = damping_force.y * 0.2
    end

    ship.applyInvariantForce(
            damping_force.x,
            damping_force.y * 3,
            damping_force.z)

    -- movement power
    local f = Vec.Zero()
    f.y = data.mass * 50 * high_ctrl

    local acc = acclerate
    local fx = acc * math.sin(-yaw)
    local fz = acc * math.cos(-yaw)

    f.x = fx * data.mass * 4 * ctrlCaches.speed_ctrl
    f.z = fz * data.mass * 4 * ctrlCaches.speed_ctrl

    ship.applyInvariantForce(f.x, f.y, f.z)
end

local function getNorthBarUV(yaw)
    local ang = (yaw + 180) % 360 - 180
    local center = math.ceil(106 + ang / 360 * 84)
    local left = center - uiData.north_bar.half_w
    local right = center + uiData.north_bar.half_w
    return left, right
end

local function DrawScreen()
    if monitor ~= nil then
        local l,r = getNorthBarUV(shipDataCached.yaw / math.pi * 180 + 180)
        SBuffer:blit(uiSprite.north_bar_border, uiData.north_bar.x-1, uiData.north_bar.y-1, colors.yellow)
        SBuffer:blit(uiSprite.north_bar_border, uiData.north_bar.x-1, uiData.north_bar.y, colors.yellow)
        SBuffer:blit(uiSprite.north_bar_border, uiData.north_bar.x-1, uiData.north_bar.y+1, colors.yellow)
        SBuffer:blit(string.sub(uiSprite.north_bar_0, l, r), uiData.north_bar.x, uiData.north_bar.y-1, colors.red)
        SBuffer:blit(string.sub(uiSprite.north_bar_1, l, r), uiData.north_bar.x, uiData.north_bar.y, colors.white)
        SBuffer:blit(string.sub(uiSprite.north_bar_2, l, r), uiData.north_bar.x, uiData.north_bar.y+1, colors.white)

        SBuffer:fill(" ", uiData.state_bar.x, uiData.state_bar.y-3, 40, 5)

        if ctrlCaches.brake then
            SBuffer:blit(string.format("[Braked]"), uiData.state_bar.x, uiData.state_bar.y-3, colors.red)
        end

        SBuffer:blit(string.format("Mass      %.2f kg", shipDataCached.mass), uiData.state_bar.x, uiData.state_bar.y - 2, colors.green)
        SBuffer:blit(string.format("Speed     %.2f m/s", shipDataCached.velocity), uiData.state_bar.x, uiData.state_bar.y - 1, colors.green)
        SBuffer:blit(string.format("Position  %.2f, %.2f, %.2f", shipDataCached.pos.x, shipDataCached.pos.y, shipDataCached.pos.z), uiData.state_bar.x, uiData.state_bar.y, colors.green)

        SBuffer:drawToScreen(monitor)

        return false
    else
        getScreen()
        return true
    end
end

local function Init()
    local face = ship.getFaceRaw()

    if face.x > 0.5 then
        face = -90
    elseif face.x < -0.5 then
        face = 90
    elseif face.z < -0.5 then
        face = 180
    elseif face.z > 0.5 then
        face = 0
    end

    yaw_off = face / 180 * math.pi

    setupScreen()
end

local function MainLoop()
    Init()

    ship.setIdle(false)

    local timer = os.startTimer(0.1)
    local eventData = {}

    while true do
        eventData = { os.pullEvent() }
        if eventData[1] == "phys_tick" then
            PhysTick(eventData[2])
        elseif eventData[1] == "timer" then
            if eventData[2] == timer then
                DrawScreen()
                timer = os.startTimer(0.1)
            end
        end
    end
end


MainLoop()